package util;

import static org.lwjgl.opengl.GL15.GL_ARRAY_BUFFER;
import static org.lwjgl.opengl.GL15.GL_STATIC_DRAW;
import static org.lwjgl.opengl.GL15.glBindBuffer;
import static org.lwjgl.opengl.GL15.glBufferData;
import static org.lwjgl.opengl.GL15.glGenBuffers;

import java.io.File;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;

import color.CLRConst;

import computation.Geometry;

import delaunay_triangulation.Delaunay_Triangulation;
import delaunay_triangulation.Point_dt;
import delaunay_triangulation.Triangle_dt;

import primatives.PointND;
import triangulation.DelaunayMapping;
import triangulation.SimpleTriangulation;
import ui.StrikeDip2DFrame.ClusterType;

public class Resampler
{
	public static String EXT_XYZ = ".xyz";
	public static String EXT_ASC = ".asc";
	public static String EXT_TXT = ".txt";
	
	// place all the points into buckets by converting the points into integer form (thus truncating).
	public static ArrayList<PointND> reduce_buckets(ArrayList<PointND> points)
	{
		ArrayList<PointND> ret = new ArrayList<PointND>();
		
		HashSet<Integer> x_comp = new HashSet<Integer>(); // quick way to check if we have the x or y comp of a point
		HashSet<Integer> y_comp = new HashSet<Integer>(); 
		
		HashMap<String, ArrayList<Double>> buckets = new HashMap<String, ArrayList<Double>>(); // str(x,y) -> list of z
		
		// first place everything into their buckets
		for (PointND p: points)
		{
			int x = (int)p.get_coord(0);
			int y = (int)p.get_coord(1);
			if (x_comp.contains(x) && y_comp.contains(y)) // we already have a point within this range so lets update the value... 
			{
				String key = x+","+y;
				buckets.get(key).add(p.get_coord(2));
				
			}
			else // a brand new point!
			{
				String key = x+","+y;
				ArrayList<Double> z_list = new ArrayList<Double>();
				z_list.add(p.get_coord(2));
				buckets.put(key, z_list);				
			}
		}
		
		// now average all the z-values (perhaps ignore the ones that are not within std?)
		for (String key: buckets.keySet())
		{
			String[] comps = key.split(",");
			int x = Integer.valueOf(comps[0]);
			int y = Integer.valueOf(comps[1]);
			
			ArrayList<Double> z_values = buckets.get(key);
			double z_avg = 0.0;
			for (Double z: z_values) z_avg+=z;
			z_avg /= z_values.size();
			
			PointND pt = new PointND(3);
			pt.set_coord(0, x);
			pt.set_coord(1, y);
			pt.set_coord(2, z_avg);
			ret.add(pt);
		}
		
		return ret;
	}
	
	
	// reduce the size of the input point set by skipping every "skip" point
	public static ArrayList<PointND> reduce(ArrayList<PointND> points, int skip)
	{
		ArrayList<PointND> sample = new ArrayList<PointND>();
		int current = 0;
		for (PointND pt : points)
		{
			if (current % skip == 0) sample.add(pt);
			current = (current + 1) % 4;
		}
		System.out.println("Reduced from " + points.size() + " to " + sample.size());
		return sample;
	}

	// scale each of the input points in the set by eps
	public static ArrayList<PointND> scale(ArrayList<PointND> points, double eps)
	{
		ArrayList<PointND> scaled = new ArrayList<PointND>();
		
		for (PointND pt : points)
		{
			PointND s = new PointND(pt.dimensions);
			s.set_coord(0, pt.get_coord(0)*eps);
			s.set_coord(1, pt.get_coord(1)*eps);
			s.set_coord(2, pt.get_coord(2)*eps);
			scaled.add(s);
		}
		return scaled;
	}
	
	// remove all points that are within thres times the standard deviation of the height
	public static ArrayList<PointND> cleanup(ArrayList<PointND> points, int thres)
	{
		ArrayList<PointND> cleaned = new ArrayList<PointND>();
		
		double std = 0.0;
		double mean = 0.0;
		double max = Double.MIN_VALUE;
		double min = Double.MAX_VALUE;
		
		// first compute the mean
		for (PointND pt : points)
		{
			double z = pt.get_coord(2);
			mean += z;
			max = Math.max(max, z);
			min = Math.min(min, z);
		}
		mean /= points.size();
		
		// now compute the std of each point
		for (PointND pt : points) std += ((pt.get_coord(2) - mean)*(pt.get_coord(2) - mean));
		std /= points.size();
		std = Math.sqrt(std);
		
		// drop all points less than the thres*std, idealy thres = 3 for 99.7% of all data
		double drop = thres*std;
		
		for (PointND pt : points)
		{
			double z = pt.get_coord(2);
			if ((z > (mean + drop) && z < (mean - drop)) || (z > (mean - drop) && z < (mean + drop))) 
			{
				cleaned.add(pt);	
			}
		}
		System.out.println("Mean="+mean+"\tSTD="+std+"\tdrop="+drop);
		System.out.println("Range="+(drop-mean)+" -> "+(drop+mean));
		
		return cleaned;
	}
	
	/* Take point cloud, compute DT. Then for each triangle compute strike and dip.
	 * Using the strike and dip convert them to (x,y) coordinates for a cylinder with the z axis as the depth.
	 * Save x', y', z, strike, dip coordinates.
	 */
	public static ArrayList<PointND> convert_to_strikedipAxis(ArrayList<PointND> pointCloud)
	{
		ArrayList<PointND> converted = new ArrayList<PointND>();
		
		// first compute the DT
		System.out.print("Triangulating...");
		Point_dt[] points = new Point_dt[pointCloud.size()];
		for (int i = 0; i < points.length; i++)
		{
			double x = pointCloud.get(i).get_coord(0);
			double y = pointCloud.get(i).get_coord(1);
			double z = pointCloud.get(i).get_coord(2);
			
			points[i] = new Point_dt(x, y, z);
		}
 
		Delaunay_Triangulation delaunay = new Delaunay_Triangulation(points);
		System.out.println("   done!");

		System.out.print("Computing new point cloud ...");
		double diameter = 100.0;
		double radius = diameter/2.0;
		Iterator<Triangle_dt> iter = delaunay.trianglesIterator();
		while (iter.hasNext())
		{
			Triangle_dt dt = iter.next();
			Point_dt v0 = dt.p1();
			Point_dt v1 = dt.p2();
			Point_dt v2 = dt.p3();
			if (v0 != null && v1 != null && v2 != null)
			{
				float[] normal = Geometry.computeSurfaceNormal(v0, v1, v2);

				double[] strike_n_dip = Geometry.computeStrikeNDipAngle2(normal, -1.0, -1.0); // TODO: input in the camera head angles
				
				double StrikeDeg = strike_n_dip[0];
				double DipDeg = strike_n_dip[1];
				
				if (!Double.isNaN(DipDeg) && !Double.isNaN(StrikeDeg))
				{
					PointND pt = new PointND(5);
					
	    			double  scale = (DipDeg/90.0)*radius;
					
	    			double x = radius + scale*Math.cos(Math.toRadians(StrikeDeg));
	    			double y = radius + scale*Math.sin(Math.toRadians(StrikeDeg));
	    			
					float[] triangleCentre = new float[3];
					triangleCentre[0] = (float)((v0.x() + v1.x() + v2.x())/3);
					triangleCentre[1] = (float)((v0.y() + v1.y() + v2.y())/3);
					triangleCentre[2] = (float)((v0.z() + v1.z() + v2.z())/3);
					
					pt.set_coord(0, x);
					pt.set_coord(1, y);
					pt.set_coord(2, triangleCentre[2]);
					pt.set_coord(3, StrikeDeg);
					pt.set_coord(4, DipDeg);
					
					converted.add(pt);
				}
			}
		}
		System.out.println(" done!");
		return converted;
	}
	
	/* Take point cloud and convert to a cylinder representation.
	 * x = radius
	 * y = height
	 * strike = degrees
	 * dim = dimension to use for the z-axis (aka: height of the cylinder)
	 */
	public static ArrayList<PointND> convert_to_cylinder(ArrayList<PointND> pointCloud, int dim)
	{
		ArrayList<PointND> converted = new ArrayList<PointND>();
		
		// first compute the DT
		System.out.print("Triangulating...");
		Point_dt[] points = new Point_dt[pointCloud.size()];
		for (int i = 0; i < points.length; i++)
		{
			double x = pointCloud.get(i).get_coord(0);
			double y = pointCloud.get(i).get_coord(1);
			double z = pointCloud.get(i).get_coord(2);
			
			points[i] = new Point_dt(x, y, z);
		}
 
		Delaunay_Triangulation delaunay = new Delaunay_Triangulation(points);
		System.out.println("   done!");

		double diameter = 200.0;
		double cylinder_height = 300;
		Point_dt bb_max = delaunay.bb_max();
		
		System.out.print("Computing new point cloud ...");
		Iterator<Triangle_dt> iter = delaunay.trianglesIterator();
		while (iter.hasNext())
		{
			Triangle_dt dt = iter.next();
			Point_dt v0 = dt.p1();
			Point_dt v1 = dt.p2();
			Point_dt v2 = dt.p3();
			if (v0 != null && v1 != null && v2 != null)
			{
				float[] normal = Geometry.computeSurfaceNormal(v0, v1, v2);

				double[] strike_n_dip = Geometry.computeStrikeNDipAngle2(normal, -1.0, -1.0); // TODO: input in the camera head angles
				
				double StrikeDeg = strike_n_dip[0];
				double DipDeg = strike_n_dip[1];
				
				if (!Double.isNaN(DipDeg) && !Double.isNaN(StrikeDeg))
				{
					PointND pt = new PointND(5);	    			
					float[] triangleCentre = new float[3];
					triangleCentre[0] = (float)((v0.x() + v1.x() + v2.x())/3);
					triangleCentre[1] = (float)((v0.y() + v1.y() + v2.y())/3);
					triangleCentre[2] = (float)((v0.z() + v1.z() + v2.z())/3);
					
					
					double R = diameter/2;
//					double scale = (triangleCentre[0]/bb_max.x())*R;
					double scale = R*(DipDeg/90.0);
					double x = R + scale*Math.cos(Math.toRadians(StrikeDeg)); 
					double y = R + scale*Math.sin(Math.toRadians(StrikeDeg));
					double depth = triangleCentre[dim];
					
					pt.set_coord(0, x);
					pt.set_coord(1, y);
					pt.set_coord(2, depth);
					
					pt.set_coord(3, StrikeDeg);
					pt.set_coord(4, DipDeg);
					
					converted.add(pt);
				}
			}
		}
		System.out.println(" done!");
		return converted;
	}
	
	
	/* General convert to cylinder cloud method which can take in some factors
	 * 
	 */
	public static ArrayList<PointND> convert_to_cylinder(SimpleTriangulation triangulation, HashMap<String, String> factors)
	{
		ArrayList<PointND> converted = new ArrayList<PointND>();
		
		boolean bStrikeDip = false;
		boolean bCurvature = false;
		boolean bNormal = false;
		
		int dim = 0;
		
		
		if (factors.containsKey("SnD"))
		{
			dim = Integer.parseInt(factors.get("SnD"));
			bStrikeDip = true;
		}
		
		if (factors.containsKey("Curv"))
		{
			bCurvature = true;
		}
		
		
		for (GeoTriangle gt: triangulation.mapping.triangles)
		{
			GeoVertex gv0 = triangulation.mapping.getVertex(gt.vertexkeys[0], triangulation.CURRENT_SMOOTHNESS_LEVEL);
			GeoVertex gv1 = triangulation.mapping.getVertex(gt.vertexkeys[1], triangulation.CURRENT_SMOOTHNESS_LEVEL);
			GeoVertex gv2 = triangulation.mapping.getVertex(gt.vertexkeys[2], triangulation.CURRENT_SMOOTHNESS_LEVEL);
			float[] normal = Geometry.computeSurfaceNormal(gv0, gv1, gv2);
			
			// use strike as angle around circle and the dip as the distance away from the center
			if (bStrikeDip)
			{
				double[] strike_n_dip = Geometry.computeStrikeNDipAngle2(normal, triangulation.dLaserStrike, triangulation.dLaserDip); // TODO: input in the camera head angles
				
				double StrikeDeg = strike_n_dip[0];
				double DipDeg = strike_n_dip[1];
				
				if (!Double.isNaN(DipDeg) && !Double.isNaN(StrikeDeg))
				{
					PointND pt = new PointND(5);	    			
					float[] triangleCentre = Geometry.computeTriangleCentre(gv0, gv1, gv2);
					
					double scale = 100*(DipDeg/90.0);
					double x = 100 + scale*Math.cos(Math.toRadians(StrikeDeg)); 
					double y = 100 + scale*Math.sin(Math.toRadians(StrikeDeg));
					double depth = triangleCentre[dim];
					
					pt.set_coord(0, x);
					pt.set_coord(1, y);
					pt.set_coord(2, depth);
					
					pt.set_coord(3, StrikeDeg);
					pt.set_coord(4, DipDeg);
					
					converted.add(pt);
				}
			}
		}
		
		return converted;
	}
	
	/* Take point cloud and convert to a torus representation.
	 * u,v = dip, strike
	 * R = distance from centre of tube to centre of torus
	 * r = radius of tube
	 * x(u,v) = (R + r cos v) cos u
	 * y(u,v) = (R + r cos v) sin u
	 * z(u,v) = r sin v
	 */
	public static ArrayList<PointND> convert_to_torus(ArrayList<PointND> pointCloud)
	{
		ArrayList<PointND> converted = new ArrayList<PointND>();
		
		// first compute the DT
		System.out.print("Triangulating...");
		Point_dt[] points = new Point_dt[pointCloud.size()];
		for (int i = 0; i < points.length; i++)
		{
			double x = pointCloud.get(i).get_coord(0);
			double y = pointCloud.get(i).get_coord(1);
			double z = pointCloud.get(i).get_coord(2);
			
			points[i] = new Point_dt(x, y, z);
		}
 
		Delaunay_Triangulation delaunay = new Delaunay_Triangulation(points);
		System.out.println("   done!");

		double diameter = 100.0;
		double cylinder_height = 300;
		Point_dt bb_max = delaunay.bb_max();
		
		System.out.print("Computing new point cloud ...");
		Iterator<Triangle_dt> iter = delaunay.trianglesIterator();
		while (iter.hasNext())
		{
			Triangle_dt dt = iter.next();
			Point_dt v0 = dt.p1();
			Point_dt v1 = dt.p2();
			Point_dt v2 = dt.p3();
			if (v0 != null && v1 != null && v2 != null)
			{
				float[] normal = Geometry.computeSurfaceNormal(v0, v1, v2);

				double[] strike_n_dip = Geometry.computeStrikeNDipAngle2(normal, -1.0, -1.0); // TODO: input in the camera head angles
				
				double StrikeDeg = strike_n_dip[0];
				double DipDeg = strike_n_dip[1];
				
				if (!Double.isNaN(DipDeg) && !Double.isNaN(StrikeDeg))
				{
					PointND pt = new PointND(5);	    			
					float[] triangleCentre = new float[3];
					triangleCentre[0] = (float)((v0.x() + v1.x() + v2.x())/3);
					triangleCentre[1] = (float)((v0.y() + v1.y() + v2.y())/3);
					triangleCentre[2] = (float)((v0.z() + v1.z() + v2.z())/3);
					
					double R = diameter;
					double r = diameter/2;
					
//					double Rs = R + R*triangleCentre[0]/bb_max.x();
//					double rs = r + r*triangleCentre[1]/bb_max.y();
//					
//					double u = StrikeDeg;
//					double v = DipDeg;
//					
//					double x = (Rs + rs * Math.cos(Math.toRadians(v)))*Math.cos(Math.toRadians(u)); 
//					double y = (Rs + rs * Math.cos(Math.toRadians(v)))*Math.sin(Math.toRadians(u));
//					double z = rs * Math.sin(v);
					
					
					double x = (R + r * Math.cos(Math.toRadians(StrikeDeg)*(triangleCentre[0]/bb_max.x())))*Math.cos(Math.toRadians(DipDeg)*triangleCentre[1]/bb_max.y());
					double y = (R + r * Math.cos(Math.toRadians(StrikeDeg)*(triangleCentre[0]/bb_max.x())))*Math.sin(Math.toRadians(DipDeg)*triangleCentre[1]/bb_max.y());
					double z = r * Math.sin(StrikeDeg);
					
					pt.set_coord(0, x);
					pt.set_coord(1, y);
					pt.set_coord(2, z);
					
					pt.set_coord(3, StrikeDeg);
					pt.set_coord(4, DipDeg);
					
					converted.add(pt);
				}
			}
		}
		System.out.println(" done!");
		return converted;
	}
	
	
	public static ArrayList<PointND> epsilonNet_reduction(SimpleTriangulation triangulation, String type)
	{
		ArrayList<PointND> epsNet = Geometry.computeEpsNet(triangulation.mapping, type, 1.0, 0.01); // TODO: input parameters for later..
		return epsNet;
	}
	
	public static void pointcloud_reduce_by_buckets(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> bucket_reduction = Resampler.reduce_buckets(points);
		PointCloudWriter.WriteFile_PointND(dir+filename+"_buckets"+output_ext, bucket_reduction);
	}
	
 	public static void pointcloud_reduction(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> subset_512 = Resampler.reduce(points, 4);
		ArrayList<PointND> subset_256 = Resampler.reduce(subset_512, 4);
		PointCloudWriter.WriteFile_PointND(dir+filename+"_512"+output_ext, subset_512);
		PointCloudWriter.WriteFile_PointND(dir+filename+"_256"+output_ext, subset_256);
	}
	
	public static void pointcloud_scaling(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> scaled = Resampler.scale(points, 0.1);
		PointCloudWriter.WriteFile_PointND(dir+filename+"_scaled"+output_ext, scaled);
	}
	
	public static void pointcloud_cleaning(String dir, String filename)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+".txt");
		ArrayList<PointND> scaled = Resampler.cleanup(points, 3);
		PointCloudWriter.WriteFile_PointND(dir+filename+"_cleaned.txt", scaled);		
	}
	
	public static void pointcloud_snd_conversion(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> converted = Resampler.convert_to_strikedipAxis(points);
		PointCloudWriter.WriteFile_PointND(dir+"sndAxis_"+filename+output_ext, converted);
	}
	
	public static void pointcloud_cylinder_conversion(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> converted = Resampler.convert_to_cylinder(points, 1);
		PointCloudWriter.WriteFile_PointND(dir+"cylinder_"+filename+output_ext, converted);
	}
	
	public static void pointcloud_torus_conversion(String dir, String filename, String input_ext, String output_ext)
	{
		ArrayList<PointND> points = PointCloudReader.ReadFile(dir+filename+input_ext);
		ArrayList<PointND> converted = Resampler.convert_to_torus(points);
		PointCloudWriter.WriteFile_PointND(dir+"torus_"+filename+output_ext, converted);
	}
	
	public static void pointcloud_epsNet_reduction(String dir, String filename, String input_ext, String output_ext)
	{
		
	}
	
	
	public static void main(String args[])
	{
		// linux lab machine
//		String dir = File.separatorChar+"home2bak"+File.separatorChar+"plai2"+File.separatorChar+"datasets"+File.separatorChar+"limestone"+File.separatorChar;
//		String filename = "scan";
		
		// windows machine
//		String dir = "D:"+File.separatorChar+"SCHOOL"+File.separatorChar+"DATA"+File.separatorChar+"limestone"+File.separatorChar+"ascii"+File.separatorChar+"merged"+File.separatorChar;
		String dir = "D:"+File.separatorChar+"SCHOOL"+File.separatorChar+"DATA"+File.separatorChar+"shatter_cones"+File.separatorChar+"campus"+File.separatorChar;
//		String dir = "/home2bak/plai2/datasets/shatter cones/bike_path/256/";
//		String dir = "/home2bak/plai2/datasets/smoothed_subset/";
		
//		for (int i=1; i<=4; i++)
//		{
////			pointcloud_reduce_by_buckets(dir, filename+i, EXT_XYZ, EXT_TXT);
////			pointcloud_scaling(dir, filename+i+"_buckets", EXT_TXT, EXT_TXT);
////			pointcloud_reduction(dir, filename+i+"_buckets_scaled_256", EXT_TXT, EXT_TXT);
//			pointcloud_conversion(dir, filename+i+"_buckets_scaled_256", EXT_TXT, EXT_TXT);
//		}
		
		pointcloud_scaling(dir, "CLOSE_UP_e0_01_s_3", "", "");
		
//		File folder = new File(dir);
//		for (File f: folder.listFiles())
//		{
//			if (f.isFile())
//			{
////				pointcloud_cylinder_conversion(dir, f.getName(), "", "");
//				pointcloud_torus_conversion(dir, f.getName(), "", "");
//			}
//		}
		
		
		
	}
	
}



